﻿#include "TesMatrix4x4.h"
#include "TesMatrix3x3.h"
#include "TesVector3.h"
#include "TesVector4.h"

void TesMatrix4x4::Zero()
{
	for (int i = 0; i < 16; ++i)
	{
		m4x4[i] = 0.0f;
	}
}

TesMatrix4x4::TesMatrix4x4()
{
	Zero();
}

void TesMatrix4x4::SetMatrix(float m[16])
{
	for (int i = 0; i < 16; ++i)
	{
		m4x4[i] = m[i];
	}
}

TesMatrix4x4::TesMatrix4x4(float m[16])
{
	SetMatrix(m);
}

void TesMatrix4x4::SetMatrix(const float m[16])
{
	for (int i = 0; i < 16; ++i)
	{
		m4x4[i] = m[i];
	}
}

TesMatrix4x4::TesMatrix4x4(const float m[16])
{
	SetMatrix(m);
}

void TesMatrix4x4::Identity()
{
	Zero();
	M11_4 = M22_4 = M33_4 = M44_4 = 1.0f;
}

void TesMatrix4x4::SetMatrix(const TesMatrix3x3& matrix3x3)
{
	Zero();
	M44_4 = 1.0f;
  M11_4 = matrix3x3.M11_3;
  M12_4 = matrix3x3.M12_3;
  M13_4 = matrix3x3.M13_3;
  M21_4 = matrix3x3.M21_3;
  M22_4 = matrix3x3.M22_3;
  M23_4 = matrix3x3.M23_3;
  M31_4 = matrix3x3.M31_3;
  M32_4 = matrix3x3.M32_3;
  M33_4 = matrix3x3.M33_3;
}

TesMatrix4x4::TesMatrix4x4(const TesMatrix3x3& matrix3x3)
{
	SetMatrix(matrix3x3);
}

TesMatrix4x4& TesMatrix4x4::operator = (const TesMatrix4x4& that)
{
	for (int i = 0; i < 16; ++i)
	{
		this->m4x4[i] = that.m4x4[i];
	}
	return *this;
}

TesMatrix4x4 TesMatrix4x4::operator + (const TesMatrix4x4& that)
{
	TesMatrix4x4 Res;
	for (int i = 0; i < 16; ++i)
	{
		Res.m4x4[i] = this->m4x4[i] + that.m4x4[i];
	}
	return Res;
}

TesMatrix4x4 TesMatrix4x4::operator - (const TesMatrix4x4& that)
{
	TesMatrix4x4 Res;
	for (int i = 0; i < 16; ++i)
	{
		Res.m4x4[i] = this->m4x4[i] - that.m4x4[i];
	}
	return Res;
}

TesMatrix4x4 TesMatrix4x4::operator * (const float& K) const
{
	TesMatrix4x4 Res;
	for (int i = 0; i < 16; ++i)
	{
		Res.m4x4[i] = this->m4x4[i] * K;
	}
	return Res;
}

TesMatrix4x4 TesMatrix4x4::operator / (const float& K)
{
	TesMatrix4x4 Res;
	for (int i = 0; i < 16; ++i)
	{
		Res.m4x4[i] = this->m4x4[i] / K;
	}
	return Res;
}

TesMatrix4x4 TesMatrix4x4::operator * (const TesMatrix4x4& that)
{
	TesMatrix4x4 Res;

	Res.M11_4 = M11_4 * that.M11_4 + M12_4 * that.M21_4 + M13_4 * that.M31_4 + M14_4 * that.M41_4;
	Res.M12_4 = M11_4 * that.M12_4 + M12_4 * that.M22_4 + M13_4 * that.M32_4 + M14_4 * that.M42_4;
	Res.M13_4 = M11_4 * that.M13_4 + M12_4 * that.M23_4 + M13_4 * that.M33_4 + M14_4 * that.M43_4;
	Res.M14_4 = M11_4 * that.M14_4 + M12_4 * that.M24_4 + M13_4 * that.M34_4 + M14_4 * that.M44_4;

	Res.M21_4 = M21_4 * that.M11_4 + M22_4 * that.M21_4 + M23_4 * that.M31_4 + M24_4 * that.M41_4;
	Res.M22_4 = M21_4 * that.M12_4 + M22_4 * that.M22_4 + M23_4 * that.M32_4 + M24_4 * that.M42_4;
	Res.M23_4 = M21_4 * that.M13_4 + M22_4 * that.M23_4 + M23_4 * that.M33_4 + M24_4 * that.M43_4;
	Res.M24_4 = M21_4 * that.M14_4 + M22_4 * that.M24_4 + M23_4 * that.M34_4 + M24_4 * that.M44_4;

	Res.M31_4 = M31_4 * that.M11_4 + M32_4 * that.M21_4 + M33_4 * that.M31_4 + M34_4 * that.M41_4;
	Res.M32_4 = M31_4 * that.M12_4 + M32_4 * that.M22_4 + M33_4 * that.M32_4 + M34_4 * that.M42_4;
	Res.M33_4 = M31_4 * that.M13_4 + M32_4 * that.M23_4 + M33_4 * that.M33_4 + M34_4 * that.M43_4;
	Res.M34_4 = M31_4 * that.M14_4 + M32_4 * that.M24_4 + M33_4 * that.M34_4 + M34_4 * that.M44_4;

	Res.M41_4 = M41_4 * that.M11_4 + M42_4 * that.M21_4 + M43_4 * that.M31_4 + M44_4 * that.M41_4;
	Res.M42_4 = M41_4 * that.M12_4 + M42_4 * that.M22_4 + M43_4 * that.M32_4 + M44_4 * that.M42_4;
	Res.M43_4 = M41_4 * that.M13_4 + M42_4 * that.M23_4 + M43_4 * that.M33_4 + M44_4 * that.M43_4;
	Res.M44_4 = M41_4 * that.M14_4 + M42_4 * that.M24_4 + M43_4 * that.M34_4 + M44_4 * that.M44_4;

	return Res;
}

TesMatrix4x4 TesMatrix4x4::Transpose()
{
	TesMatrix4x4 Res;
  Res.M11_4 = M11_4; Res.M12_4 = M21_4; Res.M13_4 = M31_4; Res.M14_4 = M41_4;
  Res.M21_4 = M12_4; Res.M22_4 = M22_4; Res.M23_4 = M32_4; Res.M24_4 = M42_4;
  Res.M31_4 = M13_4; Res.M32_4 = M23_4; Res.M33_4 = M33_4; Res.M34_4 = M43_4;
  Res.M41_4 = M14_4; Res.M42_4 = M24_4; Res.M43_4 = M34_4; Res.M44_4 = M44_4;
	return Res;
}

TesMatrix4x4 TesMatrix4x4::SetScale(float x, float y, float z)
{
	this->Identity();
	M11_4 = x; M22_4 = y; M33_4 = z;
	return *this;
}

TesMatrix4x4 TesMatrix4x4::SetTransform(const TesMatrix3x3& M3x3)
{
	SetMatrix(M3x3);
	return *this;
}

TesMatrix4x4 TesMatrix4x4::SetRotationXAxis(float Redus)
{
	TesMatrix3x3 M3x3;
	M3x3.SetRotationXAxis(Redus);
	this->SetMatrix(M3x3);
	return *this;
}

TesMatrix4x4 TesMatrix4x4::SetRotationYAxis(float Redus)
{
	TesMatrix3x3 M3x3;
	M3x3.SetRotationYAxis(Redus);
	this->SetMatrix(M3x3);
	return *this;
}

TesMatrix4x4 TesMatrix4x4::SetRotationZAxis(float Redus)
{
	TesMatrix3x3 M3x3;
	M3x3.SetRotationZAxis(Redus);
	this->SetMatrix(M3x3);
	return *this;
}

TesMatrix4x4 TesMatrix4x4::SetRotation(float Redus, const TesVector3& Axis)
{
	TesMatrix3x3 M3x3;
	M3x3.SetRotation(Redus, Axis);
	this->SetMatrix(M3x3);
	return *this;
}

TesMatrix4x4 TesMatrix4x4::SetTranslation(float x, float y, float z)
{
	Identity();
	M41_4 = x;
	M42_4 = y;
	M43_4 = z;
	return *this;
}


void TesMatrix4x4::_GetToArray(float matrix[16])
{
	for (int i = 0; i < 16; ++i)
	{
		matrix[i] = m4x4[i];
	}
}

TesMatrix3x3 GetMatrix3x3(const TesMatrix4x4& matrix, int Row, int Column)
{
	TesMatrix3x3 Res;
	int count = 0;
	for (int i = 1; i <= 4; ++i)
	{
		if (i == Column)
			continue;
		for (int j = 1; j <= 4; ++j)
		{
			if (j == Row)
				continue;
			int temp = PosTOIndex4(j, i);
			Res.m3x3[count] = matrix.m4x4[PosTOIndex4(j, i)];
			count++;
		}
	}
	return Res;
}

float TesMatrix4x4::Determinant()
{
	float TempNum[4] = {};
	TempNum[0] = _Cofactor(1, 1);
	TempNum[1] = _Cofactor(2, 1);
	TempNum[2] = _Cofactor(3, 1);
	TempNum[3] = _Cofactor(4, 1);
	return (TempNum[0] * M11_4 + TempNum[1] * M12_4 + TempNum[2] * M13_4 + TempNum[3] * M14_4);
}

float TesMatrix4x4::_Cofactor(int r, int c)
{
	float Factor = powf(-1.0f, float(r + c));
	assert(r >= 1 || r <= 4 || c >= 1 || c <= 4);//四维矩阵取行列不合法
	TesMatrix3x3 Matrix3 = GetMatrix3x3(*this, r, c);
	return Factor * Matrix3.Determinant();
}

TesMatrix4x4 TesMatrix4x4::_AdjoinMatrix()
{
	TesMatrix4x4 Res;
	int count = 0;
	for (int column = 1; column <= 4; ++column)
	{
		for (int row = 1; row <= 4; ++row)
		{
			Res.m4x4[count] = _Cofactor(row, column);
			count++;
		}
	}
	Res = Res.Transpose();
	return Res;
}

TesMatrix4x4 TesMatrix4x4::Inverse()
{
	float Det = this->Determinant();
	assert(Det != 0.0f);  //求逆矩阵的时候行列式的值不能为零,可以用增广矩阵法手动计算
	TesMatrix4x4 Res;
	Res = this->_AdjoinMatrix();
	Res = Res / Det;
	return Res;
}

TesMatrix4x4 TesMatrix4x4::SetW(float Num)
{
	M44_4 = Num;
	return *this;
}

float& TesMatrix4x4::operator[](int Index)
{
	assert(Index >= 0 && Index <= 15);
	return m4x4[Index];
}

TesVector3 operator * (const TesVector3& v, const TesMatrix4x4& matrix)
{
	TesVector3 Res;
	Res.x = v.x * matrix.M11_4 + v.y * matrix.M21_4 + v.z * matrix.M31_4 + matrix.M41_4;
	Res.y = v.x * matrix.M12_4 + v.y * matrix.M22_4 + v.z * matrix.M32_4 + matrix.M42_4;
	Res.z = v.x * matrix.M13_4 + v.y * matrix.M23_4 + v.z * matrix.M33_4 + matrix.M43_4;
	return Res;
}

TesVector4 operator * (const TesVector4& v, const TesMatrix4x4& matrix)
{
	TesVector4 Res;
	Res.x = v.x * matrix.M11_4 + v.y * matrix.M21_4 + v.z * matrix.M31_4 + v.w * matrix.M41_4;
	Res.y = v.x * matrix.M12_4 + v.y * matrix.M22_4 + v.z * matrix.M32_4 + v.w * matrix.M42_4;
	Res.z = v.x * matrix.M13_4 + v.y * matrix.M23_4 + v.z * matrix.M33_4 + v.w * matrix.M43_4;
	Res.w = v.x * matrix.M14_4 + v.y * matrix.M24_4 + v.z * matrix.M34_4 + v.w * matrix.M44_4;
	return Res;
}

TesMatrix4x4 operator * (const float& K, const TesMatrix4x4& matrix)
{
	return matrix * K;
}





